/**
 * @file main.cpp
 * @author xxx (xxx@xxx.org)
 * @brief carla-autoware 话题转发节点
 * @version 1.0
 * @date 2024-05-20
 * 
 * @copyright Copyright (c) 2024 xxx.cn. All rights reserved.
 * 
 */
#include <ros/ros.h>
#include <iostream>
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/PointCloud2.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/Twist.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

// 定义接收者
ros::Subscriber imu_sub;
ros::Subscriber lidar_sub;
ros::Subscriber twist_sub;
// 定义发送者
ros::Publisher imu_adv;
ros::Publisher lidar_adv;
ros::Publisher twist_adv;
// 定义话题名称
const std::string IMU_TOPIC = "/carla/ego_vehicle/imu"; // 这些是carla ros bridge发出的ros topic
const std::string LIDAR_TOPIC = "/carla/ego_vehicle/lidar";
const std::string TWIST_TOPIC = "/twist_cmd"; // 订阅autoware的控制指令，TwistStamped，把它转成carla ros bridge的控制指令Twist

geometry_msgs::Twist m_twist;

// imu数据回调
void imu_callback(const sensor_msgs::Imu::Ptr &imu_msg) 
{
    imu_msg->header.frame_id = "imu"; // 更换frame名称
    imu_msg->header.stamp = ros::Time::now();
    imu_adv.publish(*imu_msg);
}

// lidar数据回调
void lidar_callback(const sensor_msgs::PointCloud2::Ptr &lidar_msg) 
{
    lidar_msg->header.frame_id = "velodyne";
    lidar_msg->header.stamp = ros::Time::now();
    lidar_adv.publish(*lidar_msg);
}

// autoware发出的车辆控制指令，需要转化为ros bridge需要的geometry_msgs::Twist类型
// TwistStamped数据回调
void twist_callback(const geometry_msgs::TwistStamped::Ptr &twist_msg) 
{
    m_twist.linear = twist_msg->twist.linear;
    m_twist.angular = twist_msg->twist.angular;

    twist_adv.publish(m_twist);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "topic_transform");
    ros::NodeHandle n;
    std::cout << "topic_tranform is running!" << std::endl;

    imu_sub = n.subscribe(IMU_TOPIC, 10000, imu_callback);
    lidar_sub = n.subscribe(LIDAR_TOPIC, 10000, lidar_callback);
    twist_sub = n.subscribe(TWIST_TOPIC, 10, twist_callback);

    imu_adv = n.advertise<sensor_msgs::Imu>("/imu_raw", 10000);
    lidar_adv = n.advertise<sensor_msgs::PointCloud2>("/points_raw", 10000);
    // ros bridge carla_twist_to_contral节点
    twist_adv = n.advertise<geometry_msgs::Twist>("/carla/ego_vehicle/twist", 10); 

    ros::spin();
}